// David Eberly, Geometric Tools, Redmond WA 98052
// Copyright (c) 1998-2019
// Distributed under the Boost Software License, Version 1.0.
// http://www.boost.org/LICENSE_1_0.txt
// http://www.geometrictools.com/License/Boost/LICENSE_1_0.txt
// File Version: 3.0.1 (2018/10/05)

#pragma once

#include <Mathematics/GteApprGaussian2.h>
#include <Mathematics/GteHyperellipsoid.h>
#include <Mathematics/GteProjection.h>

namespace gte
{

// The input points are fit with a Gaussian distribution.  The center C of the
// ellipsoid is chosen to be the mean of the distribution.  The axes of the
// ellipsoid are chosen to be the eigenvectors of the covariance matrix M.
// The shape of the ellipsoid is determined by the absolute values of the
// eigenvalues.  NOTE: The construction is ill-conditioned if the points are
// (nearly) collinear.  In this case M has a (nearly) zero eigenvalue, so
// inverting M can be a problem numerically.
template <typename Real>
bool GetContainer(int numPoints, Vector2<Real> const* points,
    Ellipse2<Real>& ellipse);

// Test for containment of a point inside an ellipse.
template <typename Real>
bool InContainer(Vector2<Real> const& point, Ellipse2<Real> const& ellipse);

// Construct a bounding ellipse for the two input ellipses.  The result is
// not necessarily the minimum-area ellipse containing the two ellipses.
template <typename Real>
bool MergeContainers(Ellipse2<Real> const& ellipse0,
    Ellipse2<Real> const& ellipse1, Ellipse2<Real>& merge);


template <typename Real>
bool GetContainer(int numPoints, Vector2<Real> const* points,
    Ellipse2<Real>& ellipse)
{
    // Fit the points with a Gaussian distribution.  The covariance matrix
    // is M = sum_j D[j]*U[j]*U[j]^T, where D[j] are the eigenvalues and U[j]
    // are corresponding unit-length eigenvectors.
    ApprGaussian2<Real> fitter;
    if (fitter.Fit(numPoints, points))
    {
        OrientedBox2<Real> box = fitter.GetParameters();

        // If either eigenvalue is nonpositive, adjust the D[] values so that
        // we actually build an ellipse.
        for (int j = 0; j < 2; ++j)
        {
            if (box.extent[j] < (Real)0)
            {
                box.extent[j] = -box.extent[j];
            }
        }

        // Grow the ellipse, while retaining its shape determined by the
        // covariance matrix, to enclose all the input points.  The quadratic
        // form that is used for the ellipse construction is
        //   Q(X) = (X-C)^T*M*(X-C)
        //        = (X-C)^T*(sum_j D[j]*U[j]*U[j]^T)*(X-C)
        //        = sum_j D[j]*Dot(U[j],X-C)^2
        // If the maximum value of Q(X[i]) for all input points is V^2, then a
        // bounding ellipse is Q(X) = V^2, because Q(X[i]) <= V^2 for all i.

        Real maxValue = (Real)0;
        for (int i = 0; i < numPoints; ++i)
        {
            Vector2<Real> diff = points[i] - box.center;
            Real dot[2] =
            {
                Dot(box.axis[0], diff),
                Dot(box.axis[1], diff)
            };

            Real value =
                box.extent[0] * dot[0] * dot[0] +
                box.extent[1] * dot[1] * dot[1];

            if (value > maxValue)
            {
                maxValue = value;
            }
        }

        // Arrange for the quadratic to satisfy Q(X) <= 1.
        ellipse.center = box.center;
        for (int j = 0; j < 2; ++j)
        {
            ellipse.axis[j] = box.axis[j];
            ellipse.extent[j] = std::sqrt(maxValue / box.extent[j]);
        }
        return true;

    }

    return false;
}

template <typename Real>
bool InContainer(Vector2<Real> const& point, Ellipse2<Real> const& ellipse)
{
    Vector2<Real> diff = point - ellipse.center;
    Vector2<Real> standardized{
        Dot(diff, ellipse.axis[0]) / ellipse.extent[0],
        Dot(diff, ellipse.axis[1]) / ellipse.extent[1] };
    return Length(standardized) <= (Real)1;
}

template <typename Real>
bool MergeContainers(Ellipse2<Real> const& ellipse0,
    Ellipse2<Real> const& ellipse1, Ellipse2<Real>& merge)
{
    // Compute the average of the input centers.
    merge.center = ((Real)0.5)*(ellipse0.center + ellipse1.center);

    // The bounding ellipse orientation is the average of the input
    // orientations.
    if (Dot(ellipse0.axis[0], ellipse1.axis[0]) >= (Real)0)
    {
        merge.axis[0] = ((Real)0.5)*(ellipse0.axis[0] + ellipse1.axis[0]);
    }
    else
    {
        merge.axis[0] = ((Real)0.5)*(ellipse0.axis[0] - ellipse1.axis[0]);
    }
    Normalize(merge.axis[0]);
    merge.axis[1] = -Perp(merge.axis[0]);

    // Project the input ellipses onto the axes obtained by the average of the
    // orientations and that go through the center obtained by the average of
    // the centers.
    for (int j = 0; j < 2; ++j)
    {
        // Projection axis.
        Line2<Real> line(merge.center, merge.axis[j]);

        // Project ellipsoids onto the axis.
        Real min0, max0, min1, max1;
        Project(ellipse0, line, min0, max0);
        Project(ellipse1, line, min1, max1);

        // Determine the smallest interval containing the projected
        // intervals.
        Real maxIntr = (max0 >= max1 ? max0 : max1);
        Real minIntr = (min0 <= min1 ? min0 : min1);

        // Update the average center to be the center of the bounding box
        // defined by the projected intervals.
        merge.center += line.direction*(((Real)0.5)*(minIntr + maxIntr));

        // Compute the extents of the box based on the new center.
        merge.extent[j] = ((Real)0.5)*(maxIntr - minIntr);
    }

    return true;
}


}
